#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/Marker.h>

using namespace std;

vector< vector<double> > trajectoryPlan( vector< double > viapoint_x, vector< double > viapoint_y,
        double current_x, double current_y, double current_orientation,
        double current_v, double current_vomega,
        double period,
        double command_length);
vector<geometry_msgs::PoseStamped> genPathDisplay(vector< vector<double> > plan_result,
        double current_x, double current_y, double current_orientation,
        double period);
visualization_msgs::Marker genMarkerDisplay(vector< double > viapoint_x, vector< double > viapoint_y);


int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher path_pub = n.advertise<nav_msgs::Path>("path_display", 1);
    ros::Publisher marker_pub  = n.advertise<visualization_msgs::Marker>("waypoint_markers", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(30);

    vector< double > viapoint_x, viapoint_y;
    viapoint_x.push_back(0.0);
    viapoint_x.push_back(-2.0);
    viapoint_x.push_back(2.0);
    viapoint_x.push_back(0.0);
    viapoint_x.push_back(2.0);


    viapoint_y.push_back(0.0);
    viapoint_y.push_back(-1.0);
    viapoint_y.push_back(-1.0);
    viapoint_y.push_back(3.0);
    viapoint_y.push_back(5.0);

    visualization_msgs::Marker waypoints = genMarkerDisplay(viapoint_x, viapoint_y);

    //Current robot position at X axis
    double current_x =  0;
    //Current robot position at Y axis
    double current_y = 0;
    //Current robot orientation represented in radius
    double current_orientation = 0;
    //Current robot moving speed
    double current_v = 0;
    //Current robot rotation speed
    double current_vomega = 0;
    //Control period represented in second
    double period = 0.01;//moving command sent at every 0.01s
    //Number of commands generated at one time
    double command_length = 100;

    vector< vector<double> > plan_result = trajectoryPlan( viapoint_x, viapoint_y,
                                           current_x, current_y, current_orientation,
                                           current_v, current_vomega,
                                           period,
                                           command_length);
    vector<geometry_msgs::PoseStamped> poses_display = genPathDisplay(plan_result,
            current_x, current_y, current_orientation,
            period);


    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "map";
    odom_trans.child_frame_id = "base_link";

    nav_msgs::Path path_display;
    path_display.header.stamp = ros::Time::now();
    path_display.header.frame_id = "map";
    path_display.poses = poses_display;

    int i = 0;

    while (ros::ok()) {
        marker_pub.publish(waypoints);

        // update transform
        // (moving in a circle with radius)
        odom_trans.header.stamp = ros::Time::now();
        odom_trans.transform.translation.x = poses_display[i].pose.position.x;
        odom_trans.transform.translation.y = poses_display[i].pose.position.y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = poses_display[i].pose.orientation;

        //send the joint state and transform
        path_pub.publish(path_display);
        broadcaster.sendTransform(odom_trans);


        if (i < poses_display.size() - 1)
        {
            i += 1;
        }
        else
        {
            i += 0;
        }

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }

    return 0;
}

visualization_msgs::Marker genMarkerDisplay(vector< double > viapoint_x, vector< double > viapoint_y)
{
    visualization_msgs::Marker result;
    result.ns = "wayPoints";
    result.id = 0;
    result.type = 6;
    result.action = 0;
    result.lifetime = ros::Duration(0);
    result.scale.x = 0.2;
    result.scale.y = 0.2;
    result.color.r = 1.0;
    result.color.g = 0.7;
    result.color.b = 1.0;
    result.color.a = 1.0;

    vector< geometry_msgs::Point > posi_list;
    for (int i = 0; i < viapoint_x.size(); i++)
    {
        geometry_msgs::Point temp;
        temp.x = viapoint_x[i];
        temp.y = viapoint_y[i];
        temp.z = 0;
        posi_list.push_back(temp);
    }

    result.header.frame_id = "map";
    result.header.stamp = ros::Time::now();
    result.points = posi_list;
    return result;
}

vector< vector<double> > trajectoryPlan( vector< double > viapoint_x, vector< double > viapoint_y,
        double current_x, double current_y, double current_orientation,
        double current_v, double current_vomega,
        double period,
        double command_length)
{
    vector< vector<double> > plan_result;
    plan_result.clear();
    int num_points = viapoint_x.size();
    int num_segment = viapoint_x.size();
    int command_per_segment = command_length / num_segment;
    int command_remain = command_length - (command_per_segment * num_segment);
    ROS_INFO_STREAM("command_remain: " << command_remain);

    vector< double > allpoint_x, allpoint_y;
    allpoint_x.push_back(current_x);
    allpoint_y.push_back(current_y);
    for (int i = 0; i < num_points; i++)
    {
        allpoint_x.push_back(viapoint_x[i]);
        allpoint_y.push_back(viapoint_y[i]);
    }

    double robot_orientation = current_orientation;

    for (int i = 0; i < num_points; i++)
    {
        double start_x = allpoint_x[i];
        double start_y = allpoint_y[i];
        double end_x = allpoint_x[i + 1];
        double end_y = allpoint_y[i + 1];
        ROS_INFO_STREAM("start x: " << start_x);
        ROS_INFO_STREAM("start y: " << start_y);
        ROS_INFO_STREAM("end x: " << end_x);
        ROS_INFO_STREAM("end y: " << end_y);
        double deltax = end_x - start_x;
        double deltay = end_y - start_y;

        double segment_length = sqrt(deltax * deltax + deltay * deltay);
        ROS_INFO_STREAM("segment_length: " << segment_length);
        double segment_angle;
        if (deltax == 0 && deltay != 0)
        {
            segment_angle = 1.57;
        }
        else if (deltax == 0 && deltay == 0)
        {
            segment_angle = 0;
        }
        else if ( (deltax < 0 && deltay < 0) || (deltax > 0 && deltay < 0) )
        {
            segment_angle =  3.14 + atan( deltax / deltay );
        }
        else
        {
            segment_angle = atan( deltax / deltay );
        }
        ROS_INFO_STREAM("segment_angle: " << segment_angle);
        double delta_angle = segment_angle - robot_orientation;
        double init_vomega = delta_angle / period;
        double init_v = 0;
        vector<double> temp_command;
        temp_command.clear();
        temp_command.push_back(init_v);
        temp_command.push_back(init_vomega);
        ROS_INFO_STREAM("init_v: " << init_v);
        ROS_INFO_STREAM("init_vomega: " << init_vomega);
        plan_result.push_back(temp_command);

        double delta_length = segment_length / (command_per_segment - 1);
        double followup_vomega = 0;
        double followup_v = delta_length / period;
        for (int j = 0; j < command_per_segment - 1; j++)
        {
            vector<double> temp_command;
            temp_command.clear();
            temp_command.push_back(followup_v);
            temp_command.push_back(followup_vomega);
            ROS_INFO_STREAM("followup_v: " << followup_v);
            ROS_INFO_STREAM("followup_vomega: " << followup_vomega);
            plan_result.push_back(temp_command);
        }
        robot_orientation = segment_angle;
    }

    for (int i = 0; i < command_remain; i++)
    {
        vector<double> temp_command;
        temp_command.clear();
        temp_command.push_back(0);
        temp_command.push_back(0);
        plan_result.push_back(temp_command);
    }
    return plan_result;
}

vector<geometry_msgs::PoseStamped> genPathDisplay(vector< vector<double> > plan_result,
        double current_x, double current_y, double current_orientation,
        double period)
{
    ROS_INFO_STREAM("=======================================");
    vector<geometry_msgs::PoseStamped> poses_display;
    int num_points = plan_result.size();
    ROS_INFO_STREAM("num_points: " << num_points);
    double robot_posi_x = current_x;
    double robot_posi_y = current_y;
    double robot_posi_orientation = current_orientation;
    for (int i = 0; i < num_points; i++)
    {
        geometry_msgs::PoseStamped temp;
        temp.header.stamp = ros::Time::now();
        temp.header.frame_id = "map";
        temp.pose.position.x = robot_posi_x;
        temp.pose.position.y =  robot_posi_y;
        temp.pose.position.z = 0;
        temp.pose.orientation = tf::createQuaternionMsgFromYaw(-robot_posi_orientation);
        poses_display.push_back(temp);

        double v = plan_result[i][0];
        double moving_length = v * period;
        double vomega = plan_result[i][1];

        robot_posi_orientation += vomega * period;
        robot_posi_x += moving_length * sin(robot_posi_orientation);
        robot_posi_y += moving_length * cos(robot_posi_orientation);
        ROS_INFO_STREAM("robot_posi_x: " << robot_posi_x);
        ROS_INFO_STREAM("robot_posi_y: " << robot_posi_y);
    }
    return poses_display;
}


